
#include <ros/ros.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <ros/time.h>
// #include "motor_driver/controlcan.h"
#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <boost/thread.hpp>
// #include "dirve_ros-master/dirve_ctr.h"
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/TransformStamped.h>
#include <dynamic_reconfigure/server.h>
#include <tf/tf.h>
#include "sensor_msgs/JointState.h"
#include "zeus_s2_msgs/Feedback.h"
#include "zeus_s2_msgs/wheel_state.h"



#define MAX_LINEAR  1.57        //m/s
#define MAX_ANGULAR  0.7853     //red/s

#define GROUP_INT_BYTE(X,N)   ((X[N+3]<<24)+(X[N+2]<<16)+(X[N+1]<<8)+X[N])
#define  MOTOR_ID(X,N)   ((X[N]<<8 | X[N+1])>>5)

#define PID_INTEGRAL_UP    0.8   //积分上限
#define PID_SCALE      1     //PID缩放系数





class DriverOdomPub
{
   public:
        DriverOdomPub();
        ~DriverOdomPub();
        void loop();

    private:

        void handle_speed_data(void);
        void motor_feedback_callback( const zeus_s2_msgs::Feedback::ConstPtr& msg);
        void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg);

        boost::mutex cmd_vel_mutex_;
        ros::Time last_twist_time_;
        
        boost::mutex mutex_;

        ros::Subscriber cmd_sub_;
        ros::Subscriber motor_driver_sub_;
        ros::Publisher odom_pub_;
        ros::Publisher wheel_state_join_;
        zeus_s2_msgs::Feedback  left_right_wheel_state_;
        ros::Publisher speed_pid_;              //发布测试pid话题
         // zeus_s2_driver::motor_pid_vol  motor_pid_;      //调试pid参数类型

        
      //   ros::Publisher front_right_wheel_state_;
      //   os::Publisher rear_left_wheel_state_;
      //   ros::Publisher rear_right_wheel_state_;

        std::string port_name_;

        std::string odom_frame_;
        std::string base_frame_;

        bool recv_flag_;
        bool start_flag_;
        bool publish_odom_transform_;
        bool publish_odom_topic_;
        zeus_s2_msgs::Feedback current_speed_;
        geometry_msgs::Twist  current_twist_;
        nav_msgs::Odometry odom_;
        geometry_msgs::TransformStamped transformStamped_;
        tf2_ros::TransformBroadcaster br_;
        
        int fdcom;
        int baud_rate_;
        unsigned char buffer_data[100];

        int ret;
        // pthread_t threadid;
        ros::Time now_;
        ros::Time last_time_;
        // VCI_BOARD_INFO pInfo;//用来获取设备信息。
        // VCI_BOARD_INFO pInfo1 [50];
        // short error;

        double wheel_track_4wd_;                      //轮距
        double encoder_resolution_4wd_;
        double ticks_per_meter_;       //脉冲数/m

        double linear_correction_factor_;
        double angular_correction_factor_;
        double gear_reduction_4wd_;     //减速比
        double wheel_diameter_4wd_;    //直径
        double start_rotation_limit_w_;
        double delta_time_;
        double delta_xy_ave_;
        double delta_th_;
        double v_linear_;
        double v_angular_;
        double delta_x_;
        double delta_y_;

        double accumulation_x_;
        double accumulation_y_;
        double accumulation_th_;
        double accumulation_left_;           //记录轮子转动角度
        double accumulation_left_1_;
        double accumulation_right_;
        double accumulation_right_1_;

        double delta_left_;
        double delta_right_;        //里程差值 单位 m
        double delta_left_1_;
        double delta_right_1_;        //里程差值 单位 m
        int32_t rev_left_;
        int32_t rev_left_1_;
        int32_t rev_right_;       
        int32_t rev_right_1_;
        int cur_left_;              //存放脉冲计数值
        int cur_left_1_; 
        int cur_right_;
        int cur_right_1_;
        double encoder_low_wrap_;       //解决编码器溢出
        double encoder_high_wrap_;
        int l_wheel_mult_;              //多圈计数
        int l_wheel_mult_1_;              //多圈计数
        int r_wheel_mult_;
        int r_wheel_mult_1_;
        int encoder_min_;
        int encoder_max_;   

        int32_t line_speed_percent;
        int32_t angle_speed_percent;

      //   int32_t rev_left_;
      //   int32_t rev_right_;
        uint8_t queueFlag;
        uint8_t can_recv_flag;

        float motor_kp;
        float motor_ki;
        float motor_kd;
        int ele_currVol;
        float speed_l;
        float speed_r;
        short error;


        int16_t  motor_ctlVol_one;
        int16_t  motor_ctlVol_two;
        int16_t  motor_ctlVol_three;
        int16_t  motor_ctlVol_four;
        short left_des;
        short right_des;
        bool  reset_vol;
        bool  cmd_vel_flag;
        bool  rev_speed_flag;

};

